/**
 * @file SRPQ_Controller.cpp
 * @author Jozenlee (2250017028@qq.com)
 * @brief 
 * @version 0.1
 * @date 2022-03-09
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "srpq_controller/SRPQ_Controller.h"

SRPQ_Controller::SRPQ_Controller():RobotController(){ 
    // Initial params
    userParameters.initializeFromYamlFile("user_parameters");

 }

//#define RC_ESTOP
/**
 * Initializes the Control FSM.
 */
void SRPQ_Controller::initializeController() {
  // Initialize a new GaitScheduler object
  _gaitScheduler = new GaitScheduler<float>(&userParameters,  _controlParameters->controller_dt);

  // Initialize a new ContactEstimator object
  //_contactEstimator = new ContactEstimator<double>();
  ////_contactEstimator->initialize();

  // Initializes the Control FSM with all the required data
  _controlFSM = new ControlFSM<float>(_quadruped, _stateEstimator,
                                      _legController, _gaitScheduler,
                                      _desiredStateCommand, _controlParameters,
                                      &userParameters);
}

/**
 * Calculate the commands for the leg controllers using the ControlFSM logic.
 */
void SRPQ_Controller::runController() {
  // Find the current gait schedule
  _gaitScheduler->step();

  // Find the desired state trajectory
  _desiredStateCommand->convertToStateCommands();

  // Run the Control FSM code
  _controlFSM->runFSM();
}

int main(){

}